#include "test_global_planner.h"


GlobalPlanner::GlobalPlanner(){
    // 路径生成规划器
    int planner_type = ros::param::param<int>("/global_planner/planner_type", 0);
    switch(planner_type){
        case 0:
            __global_planner_type = GlobalPlannerType::A_STAR;
            __planner_ptr = boost::make_shared<AstarPlanner>();
            break;
        default:
            break;
    }
    // ROS 接口
    __path_pub = __nh.advertise<nav_msgs::Path>("/global_path_origin", 1, true);
    __path_smooth_pub = __nh.advertise<nav_msgs::Path>("/global_path_smooth", 1, true);
    __start_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/global_start_pose", 1, true);
    __goal_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/global_goal_pose", 1, true);
    __expand_points_pub = __nh.advertise<nav_msgs::OccupancyGrid>("/global_expand_points", 1, true);
    __map_sub = __nh.subscribe("/map", 10, &GlobalPlanner::MapCallback, this);
    __rviz_pose_sub = __nh.subscribe("/move_base_simple/goal", 10, &GlobalPlanner::RvizPosesCallback, this);

    __planner_ptr->SetDebugMode(false);
}

GlobalPlanner::~GlobalPlanner(){
    
}


// 地图回调函数
void GlobalPlanner::MapCallback(const nav_msgs::OccupancyGridConstPtr& msg){
    __map_ptr = msg;
    ROS_INFO("Receive a map, width=%d, height=%d", __map_ptr->info.width, __map_ptr->info.height);
    // 转换数据格式
    __map_2d.SetMap(*msg);
    if(__planner_ptr){
        __planner_ptr->SetMap2D(__map_2d);
    }
}

// Rviz位姿回调函数
void GlobalPlanner::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    if(!__map_ptr){
        ROS_ERROR("Map is not received");
        return;
    }

    if(__rviz_poses.empty()){
        ROS_INFO("----------------------RvizPosesCallback----------------------");
    }

    // 数量小于2时，追加
    if(__rviz_poses.size() < 2){
        __rviz_poses.emplace_back(*msg);
        ROS_INFO("Add a pose, x=%0.3f, y=%0.3f, yaw=%0.3f. rviz_poses size=%ld", 
            msg->pose.position.x, 
            msg->pose.position.y,
            tf2::getYaw(msg->pose.orientation),
            __rviz_poses.size()
        );
    }

    if(__rviz_poses.size() == 2){
        ROS_INFO("Start planning path");
        __start_pose_pub.publish(__rviz_poses.front());
        __goal_pose_pub.publish(__rviz_poses.back());

        // 开始规划路径
        if(__planner_ptr){
            ros::Time start_time = ros::Time::now();

            // 生成路径
            Pose2D start = msg_convert::Pose2Pose2D(__rviz_poses.front().pose);
            Pose2D goal = msg_convert::Pose2Pose2D(__rviz_poses.back().pose);
            vector<Pose2D> points;
            vector<Pixel2D> expend_points; 
            ros::Time gen_time;
            if(__planner_ptr->GeneratePath(start, goal, points, expend_points)){
                ROS_INFO_STREAM("[RvizPosesCallback] GeneratePath successfully!"); 
                PublishPath(__path_pub, points);
                PublishExpendPointsMap(expend_points);
                gen_time = ros::Time::now();

                // 路径平滑
                vector<Pose2D> smooth_points; 
                __planner_ptr->PathSmooth(points, smooth_points);
                PublishPath(__path_smooth_pub, smooth_points);

                //计算耗时
                double total_time_cost = (ros::Time::now() - start_time).toSec();
                double gen_time_cost = (gen_time - start_time).toSec();
                double smooth_time_cost = (ros::Time::now() - gen_time).toSec();
                ROS_INFO("[GlobalPlanner] gen path time cost=%0.3f, gen cost=%0.3f, smooth cost=%0.3f", 
                    total_time_cost, gen_time_cost, smooth_time_cost);
                
            }
            else
                ROS_WARN_STREAM("[RvizPosesCallback] GeneratePath failed!"); 
        }

        // 规划完成，清空位姿
        vector<geometry_msgs::PoseStamped> rviz_poses;
        __rviz_poses.swap(rviz_poses);

    }
}

// 发布原始路径
void GlobalPlanner::PublishPath(ros::Publisher& pub, vector<Pose2D>& points){
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = "map";
    path_msg.header.stamp = ros::Time::now();
    for(auto& point : points){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header = path_msg.header;
        pose_stamp.pose = msg_convert::Pose2D2Pose(point);
        // cout << "--> [path_msg] point_2f (" << point_2f.x << ", " << point_2f.y << ")"<< endl;
        pose_stamp.pose.position.z = 0.05;
        path_msg.poses.emplace_back(pose_stamp);
    }
    pub.publish(path_msg);    
}

// 发布扩展点
void GlobalPlanner::PublishExpendPointsMap(vector<Pixel2D>& expend_points){
    if(__expand_points_pub.getNumSubscribers() > 0){
        nav_msgs::OccupancyGrid grid_msg;
        grid_msg.header = __map_ptr->header;
        grid_msg.info = __map_ptr->info;
        grid_msg.data.assign(__map_ptr->data.size(), 0);
        for(auto& point : expend_points){
            long index = point.y * __map_ptr->info.width + point.x;
            if(index < 0 || index >= __map_ptr->data.size())
                continue;
            grid_msg.data[index] = 30;
        }
        __expand_points_pub.publish(grid_msg);
    }   
}

/******************************************************
 * 主函数 * 
 ******************************************************/
int main(int argc, char** argv){
    ros::init(argc, argv, "test_global_planner");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
	
    ROS_INFO("test_global_planner initializating......");
    ros::NodeHandle nh;
    GlobalPlanner global_planner;
    ros::spin();
    return 0;
}